#include "HkpRigidBody.h"

namespace Havok
{
	namespace Dynamics
	{

		hkVector4 getHkVector4(float x, float y, float z)
		{
			return hkVector4(x,y,z);
		}

		HkpRigidBody::HkpRigidBody(HkpRigidBodyCinfo^ info)
		{
			m_native = new hkpRigidBody(*info->m_info);
		}

#pragma region Mass
		void HkpRigidBody::Mass::set(float value)
		{
			this->m_native->setMass(value);
		}
		
		float HkpRigidBody::Mass::get()
		{
			return this->m_native->getMass();
		}
#pragma endregion

#pragma region Position - Rotation - Transformation

		inline void setRotation(hkpRigidBody* rigidBody, float x, float y, float z, float w)
		{
			rigidBody->setRotation(hkQuaternion(x,y,z,w));
		}

		void HkpRigidBody::Rotation::set(Quaternion value)
		{			
			setRotation(m_native,value.X, value.Y, value.Z, value.W);
		}
		
		Quaternion HkpRigidBody::Rotation::get()
		{
			const hkVector4* _w64 vec = &m_native->getRotation().getImag();

			return Quaternion(vec->getSimdAt(0),vec->getSimdAt(1),vec->getSimdAt(2),m_native->getRotation().getReal());
		}
		
		inline void setPosition(hkpRigidBody* rigidBody, float x, float y, float z)
		{
			rigidBody->setPosition(hkVector4(x,y,z));
		}

		void HkpRigidBody::Position::set(Vector3 value)
		{
			setPosition(m_native, value.X, value.Y, value.Z);
		}
		
		Vector3 HkpRigidBody::Position::get()
		{
			return Vector3(
				m_native->getPosition().getSimdAt(0),
				m_native->getPosition().getSimdAt(1),
				m_native->getPosition().getSimdAt(2));

			//return v;
		}

		Matrix HkpRigidBody::Transform::get()
		{
			Matrix mat = Matrix::Identity;
			Quaternion quat = Quaternion::Identity;
			Vector3 vec = Vector3::Zero;

			vec.X = m_native->getPosition().getSimdAt(0);
			vec.Y = m_native->getPosition().getSimdAt(1);
			vec.Z = m_native->getPosition().getSimdAt(2);

			quat.X = m_native->getRotation().getImag().getSimdAt(0);
			quat.Y = m_native->getRotation().getImag().getSimdAt(1);
			quat.Z = m_native->getRotation().getImag().getSimdAt(2);
			quat.W = m_native->getRotation().getReal();

			mat *= Matrix::CreateFromQuaternion(quat);
			mat *= Matrix::CreateTranslation(vec);

			return mat;
		}
		
#pragma endregion
		
#pragma region CenterOfMass
		
		inline void setCenterOfMassLocal(hkpRigidBody* rigidBody, float x, float y, float z)
		{
			rigidBody->setCenterOfMassLocal(hkVector4(x,y,z));
		}
		
		void HkpRigidBody::CenterOfMassLocal::set(Vector3 value)
		{
			//RigidBodyNative::setCenterOfMass(this->m_native, value.X, value.Y, value.Z);

			setCenterOfMassLocal(m_native,value.X, value.Y, value.Z);
		}
		
		Vector3 HkpRigidBody::CenterOfMassLocal::get()
		{
			return Vector3(
				m_native->getCenterOfMassLocal().getSimdAt(0),
				m_native->getCenterOfMassLocal().getSimdAt(1),
				m_native->getCenterOfMassLocal().getSimdAt(2));
		}
		
		Vector3 HkpRigidBody::CenterOfMassWorld::get()
		{
			return Vector3(
				m_native->getCenterOfMassInWorld().getSimdAt(0),
				m_native->getCenterOfMassInWorld().getSimdAt(1),
				m_native->getCenterOfMassInWorld().getSimdAt(2));
		}
		
#pragma endregion
		
#pragma region Velocity
		
		inline void setLinearVelocity(hkpRigidBody* rigidBody, float x, float y, float z)
		{
			rigidBody->setLinearVelocity(hkVector4(x,y,z));
		}

		void HkpRigidBody::LinearVelocity::set(Vector3 value)
		{
			setLinearVelocity(m_native, value.X, value.Y, value.Z);
		}
		
		Vector3 HkpRigidBody::LinearVelocity::get()
		{
			return Vector3(
				m_native->getLinearVelocity().getSimdAt(0),
				m_native->getLinearVelocity().getSimdAt(1),
				m_native->getLinearVelocity().getSimdAt(2));
		}

		inline void setAngularVelocity(hkpRigidBody* rigidBody, float x, float y, float z)
		{
			rigidBody->setAngularVelocity(hkVector4(x,y,z));
		}

		void HkpRigidBody::AngularVelocity::set(Vector3 value)
		{
			setAngularVelocity(m_native, value.X, value.Y, value.Z);
		}

		Vector3 HkpRigidBody::AngularVelocity::get()
		{
			const hkVector4* _w64 vec = &m_native->getAngularVelocity();
			return Vector3(vec->getSimdAt(0), vec->getSimdAt(1), vec->getSimdAt(2));
		}
		
#pragma endregion
		
#pragma region Damping

		void HkpRigidBody::LinearDamping::set(float value)
		{
			m_native->setLinearDamping(value);
		}

		float HkpRigidBody::LinearDamping::get()
		{
			return m_native->getLinearDamping();
		}

		void HkpRigidBody::AngularDamping::set(float value)
		{
			m_native->setAngularDamping(value);
		}

		float HkpRigidBody::AngularDamping::get()
		{
			return m_native->getAngularDamping();
		}

#pragma endregion

#pragma region Friction - Restitution

		void HkpRigidBody::Friction::set(float value)
		{
			m_native->setFriction(value);
		}

		float HkpRigidBody::Friction::get()
		{
			return m_native->getFriction();
		}

		void HkpRigidBody::Restitution::set(float value)
		{
			m_native->setRestitution(value);
		}

		float HkpRigidBody::Restitution::get()
		{
			return m_native->getRestitution();
		}

#pragma endregion

#pragma region Clipping velocities

		void HkpRigidBody::MaxLinearVelocity::set(float value)
		{
			m_native->setMaxLinearVelocity(value);
		}

		float HkpRigidBody::MaxLinearVelocity::get()
		{
			return m_native->getMaxLinearVelocity();
		}

		void HkpRigidBody::MaxAngularVelocity::set(float value)
		{
			m_native->setMaxAngularVelocity(value);
		}

		float HkpRigidBody::MaxAngularVelocity::get()
		{
			return m_native->getMaxAngularVelocity();
		}

#pragma endregion

#pragma region Apply Impulse and Force

		inline void applyLinearImpulse(hkpRigidBody* rigidBody, float x, float y, float z)
		{
			rigidBody->applyLinearImpulse(hkVector4(x,y,z));
		}

		void HkpRigidBody::ApplyLinearImpulse(Vector3 impulse)
		{
			applyLinearImpulse(m_native, impulse.X, impulse.Y, impulse.Z);
		}

		inline void applyPointImpulse(hkpRigidBody* rigidBody, float x0, float y0, float z0, float x1, float y1, float z1)
		{
			rigidBody->applyPointImpulse(hkVector4(x0,y0,z0), hkVector4(x1,y1,z1));
		}

		void HkpRigidBody::ApplyPointImpulse(Vector3 impulse, Vector3 point)
		{
			applyPointImpulse(m_native, impulse.X, impulse.Y, impulse.Z, point.X, point.Y, point.Z);
		}

		inline void applyAngularImpulse(hkpRigidBody* rigidBody, float x, float y, float z)
		{
			rigidBody->applyAngularImpulse(hkVector4(x,y,z));
		}

		void HkpRigidBody::ApplyAngularImpulse(Vector3 impulse)
		{
			applyAngularImpulse(m_native, impulse.X, impulse.Y, impulse.Z);
		}

		inline void applyForce(hkpRigidBody* rigidBody, double t, float x, float y, float z)
		{
			rigidBody->applyForce((float)t, hkVector4(x,y,z));
		}

		void HkpRigidBody::ApplyForce(GameTime^ gameTime, Vector3 force)
		{
			applyForce(m_native, gameTime->ElapsedGameTime.TotalSeconds, force.X, force.Y, force.Z);
		}

		inline void applyTorque(hkpRigidBody* rigidBody, double t, float x, float y, float z)
		{
			rigidBody->applyTorque((float)t, hkVector4(x,y,z));
		}

		void HkpRigidBody::ApplyTorque(GameTime^ gameTime, Vector3 torque)
		{
			applyTorque(m_native, gameTime->ElapsedGameTime.TotalSeconds, torque.X, torque.Y, torque.Z);
		}
#pragma endregion

		HkpRigidBody^ HkpRigidBody::GetRigidBody(Collision::HkpCollidable^ collidable)
		{
			hkpRigidBody* body = hkGetRigidBody(collidable->m_collidable);
			if (body==HK_NULL) return nullptr;
			else return gcnew HkpRigidBody(body);
		}
	}
}
